import rclpy
import cv2
import cv_bridge
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
from sensor_msgs.msg import Image
from std_msgs.msg import String
from geometry_msgs.msg import Pose
import numpy as np
from origincar_msg.msg import Sign
from std_msgs.msg import Int32
import time


modelPath = "/root/dev_ws/src/opencv_pkg_python/opencv_pkg_python/model/"

#预定义变量
color = (0, 0, 255)
thick = 3
font_scale = 0.5
font_thickness = 2
flag = 1

class QrCodeDetection(Node):
    def __init__(self):
        super().__init__('qrcode_detect')
        self.bridge = cv_bridge.CvBridge()

        # 接受来自utils/NV122BGR的imgae_out
        # self.image_sub = self.create_subscription(
            # CompressedImage, "/image_raw1", self.image_callback, 10)

        self.image_sub = self.create_subscription(Image, '/image_raw1',self.image_callback,1)

        self.pub_img = self.create_publisher(
            Image, '/qrcode_detected/img_result', 10)

        self.pub_qrcode_info = self.create_publisher(
            String, "/qrcode_detected/info_result", 10)
        self.info_result = String()

        self.pub_qrcode_pose = self.create_publisher(
            Pose, "/qrcode_detected/pose_result", 1)
        self.pose_result = Pose()

        self.publisher_1 = self.create_publisher(Int32, "/sign4return", 10)

        self.detect_obj = cv2.wechat_qrcode_WeChatQRCode(
            modelPath+'detect.prototxt', modelPath+'detect.caffemodel',
            modelPath+'sr.prototxt', modelPath+'sr.caffemodel')

        self.publisher = self.create_publisher(Sign, "sign_switch", 10)  
        self.a = 0  # 计数器
        self.timer = self.create_timer(0.2, self.timer_callback)  # 1秒执行5次

    def timer_callback(self):
        self.a += 1
        pass  # 确保定时器回调函数有内容，可以用来处理图像

    def image_callback (self,msg):
        if self.a >= 5:
            self.a=0
            # cv_image = self.bridge.compressed_imgmsg_to_cv2(msg)
            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
            # 检查转换后的图像是否为空
            if cv_image is None:
                print("错误：转换后的图像为空。")
                return

            qrInfo, qrPoints = self.detect_obj.detectAndDecode(cv_image)
            emptyList = ()
            if qrInfo != emptyList:
                # msg = String()
                # msg.data = 'OFF'5
                # self.publisher_1.publish(msg)
                msg = Int32()
                msg.data = 5  # 设置 Int32 类型的数据值
                self.publisher_1.publish(msg)  # 发布消息


                self.get_logger().info('qrInfo: "{0}"'.format(qrInfo))
                self.get_logger().info('qrPoints: "{0}"'.format(qrPoints))

                qrInfo_str = qrInfo[0]

                msg = Sign()
                if qrInfo_str == "AntiClockWise":
                    msg.sign_data = 4
                elif qrInfo_str == "ClockWise":
                    msg.sign_data = 3
                self.publisher.publish(msg)
                # 发布完标志位后关闭节点
                self.destroy_node()
                rclpy.shutdown()

def main(args=None):
    flag = 1
    rclpy.init(args=args)

    qrCodeDetection = QrCodeDetection()
    while rclpy.ok():
        rclpy.spin(qrCodeDetection)

    qrCodeDetection.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()